
#include <mp.h>
#include <debug.h>
#include <string.h>
#include <multiboot.h>
#include <devmgr.h>
#include <kmalloc.h>

static byte intmode;

/**
 * Read the MP-Spec tables and create device structures for devices found (cpu/lapic/ioapic)
 * 
 * ! kmalloc must be initialized first
 */
int init_mptables(void)
{
	int ret = 0;
	dword ebda = get_ebda_addr();
	
	intmode = MP_INTMODE_PIC;  // default
	
	mp_fp_t *fpt;
	
	// TODO: include last 1k of low mem?
	fpt = find_mp_fp((void*)ebda, (void*)ebda+0x400);
	if (fpt == NULL) 
	{
		fpt = find_mp_fp((void*)MP_FP_START, (void*)MP_FP_END);
		if (fpt == NULL) {
			debugf("[MP] Floating Pointer Not Found!\n");
			return MP_ERR_FP_NOTFOUND;
		}
	}
	
	debugf("[MP] Floating Pointer v%d @ 0x%08x %s\n", fpt->revision, fpt, 
	 (fpt->features[1] & MP_FEAT2_IMCRP ? "IMCR" : "Virtual Wire"));
	
	if (fpt->features[0] == 0)  // not a default configuration
	{
		if (fpt->addr != 0)  // check for valid address
		{
			unsigned char checksum = 0;
			unsigned char *ptr;
			int i;
			
			// read config table
			mp_config_table_t *config = (mp_config_table_t*)fpt->addr;
			
			// check the checksum
			ptr = (unsigned char*)config;
			for (i = 0; i < config->length; i++)
				checksum += *(ptr++);
			
			if (checksum == 0)  // good
			{
				bool ext = FALSE;
				
				debugf("[MP] Configuration Table (%8s) %12s @ 0x%08x\n",
				 config->oem_id, config->prod_id, config);
				
				// check checksum for extended table
				checksum = 0;
				ptr = (unsigned char*)config;
				ptr += 0x28;
				for (i = 0; i < config->ext_length; i++)
					checksum += *(ptr++);
				
				if ((config->ext_length > 0) && (checksum == 0))  // extended table found
				{
					ext = TRUE;
					debugf("[MP] Extended Configuration Table Found\n");
				}
				
				debugf("[MP] %d Entries\n", config->entry_count);
				unsigned char *entries = (unsigned char*)config + sizeof(mp_config_table_t);
				byte type = 0;
				byte cpus = 0;
				
				for (i = 0; i < config->entry_count; i++)  // enumerate table entries
				{
					type = *entries;
					if (type == MP_TYPE_CPU)
					{
						byte family, model, stepping;
						mp_config_entry_cpu_t *e = (mp_config_entry_cpu_t*)entries;
						family = (e->signature & MP_CPUSIG_FAMILY) >> 8;
						model = (e->signature & MP_CPUSIG_MODEL) >> 4;
						stepping = (e->signature & MP_CPUSIG_STEPPING);
						
						debugf("[MP] CPU %1x:%1x:%1x %s, %s\n", family, model, stepping,
						 (e->flags & MP_CPUFLAGS_ENABLE ? "ENABLED" : "DISABLED"),
						 (e->flags & MP_CPUFLAGS_BOOT ? "BOOT" : "NON-BOOT"));
						debugf("[MP] Local APIC v%d (%d)\n", e->lapic_version, e->lapic_id);
						
						// create a device structure
						device_t *dev = (device_t*)kmalloc(sizeof(device_t));
						cpu_locator_t *locator = (cpu_locator_t*)kmalloc(sizeof(cpu_locator_t));
						if ((dev != NULL) && (locator != NULL))
						{
							dev->type = DEV_TYPE_CPU;
							dev->class = DEV_CLASS_CPU;
							dev->id = "CPU";  // TODO: create a real id?
							dev->gint = 0;
							dev->handle = devmgr_create_handle();
							dev->init = dev->start = dev->stop = dev->handler = NULL;
							dev->locator = locator;
							locator->id = e->lapic_id;
							locator->mem = (void*)config->lapic_addr;
							devmgr_add_device(dev);
						}
						else
						{
							debugf("[MP] Could not allocate device structure!\n");
							pause();
						}
						
						cpus++;
						
						entries += sizeof(mp_config_entry_cpu_t);
					}
					else if (type == MP_TYPE_BUS)
					{
						mp_config_entry_bus_t *e = (mp_config_entry_bus_t*)entries;
						
						debugf("[MP] Bus (%6s) (%d)\n", e->type_string, e->id);
						
						// create a device structure
						device_t *dev = (device_t*)kmalloc(sizeof(device_t));
						bus_locator_t *locator = (bus_locator_t*)kmalloc(sizeof(bus_locator_t));
						if ((dev != NULL) && (locator != NULL))
						{
							dev->type = DEV_TYPE_BUS;
							dev->class = DEV_CLASS_UKNOWN;  // make one?
							dev->id = "BUS";  // use bus string from mp table?
							dev->gint = 0;
							dev->handle = devmgr_create_handle();
							dev->init = dev->start = dev->stop = dev->handler = NULL;
							dev->locator = locator;
							locator->id = e->id;
							devmgr_add_device(dev);
						}
						else
						{
							debugf("[MP] Could not allocate device structure\n");
							pause();
						}
						
						entries += sizeof(mp_config_entry_bus_t);
					}
					else if (type == MP_TYPE_IOAPIC)
					{
						// TODO: set intmode to APIC and get that working
						
						mp_config_entry_ioapic_t *e = (mp_config_entry_ioapic_t*)entries;
						
						debugf("[MP] I/O APIC v%d (%d) @ 0x%08x %s\n", e->version, e->id,
						 e->addr, (e->flags & MP_IOAPICFLAGS_ENABLE ? "ENABLED" : "DISABLED"));
						
						// create a device structure
						device_t *dev = (device_t*)kmalloc(sizeof(device_t));
						ioapic_locator_t *locator = (ioapic_locator_t*)kmalloc(sizeof(ioapic_locator_t));
						if ((dev != NULL) && (locator != NULL))
						{
							dev->type = DEV_TYPE_IOAPIC;
							dev->class = DEV_CLASS_SYSPERIPHERAL;
							dev->id = "I/O APIC";
							dev->gint = 0;
							dev->handle = devmgr_create_handle();
							dev->init = dev->start = dev->stop = dev->handler = NULL;
							dev->locator = locator;
							locator->mem = (void*)e->addr;
							devmgr_add_device(dev);
						}
						else
						{
							debugf("[MP] Could not allocate device structure\n");
							pause();
						}
						
						entries += sizeof(mp_config_entry_ioapic_t);
					}
					else if (type == MP_TYPE_IOINT)
					{
						mp_config_entry_ioint_t *e = (mp_config_entry_ioint_t*)entries;
						
						// TODO: maybe not print this...
						//debugf("[MP] I/O APIC Interrupt (%x) (%d,%d)->(%d,%d)\n", e->int_type,
						// e->src_bus_id, e->src_bus_irq, e->dest_ioapic_id, e->dest_ioapic_id);
						(void)e;
						
						entries += sizeof(mp_config_entry_ioint_t);
					}
					else if (type == MP_TYPE_LINT)
					{
						mp_config_entry_lint_t *e = (mp_config_entry_lint_t*)entries;
						
						debugf("[MP] Local APIC Interrupt (%x) (%d,%d)->(%d,%d)\n", e->int_type,
						 e->src_bus_id, e->src_bus_irq, e->dest_lapic_id, e->dest_lapic_int);
						(void)e;
						
						entries += sizeof(mp_config_entry_lint_t);
					}
					else
					{
						debugf("[MP] Invalid Entry Type %x\n", type);
						break;
					} 
				}
			}
			else
			{
				debugf("[MP] Invalid MP Config Table @ 0x%08x\n", config);
				ret = MP_ERR_INVALID_CHECKSUM;
			}
		}
		else
		{
			debugf("[MP] Invalid Address for MP Config Table\n");
			ret = MP_ERR_INVALID_ADDR;
		}
	}
	else
	{
		// some default config setup
		debugf("[MP] Default Config #%d\n", fpt->features[0]);
	}
	
	
	return ret;
}

/**
 * Search for the MP spec Floating Pointer structure in memeory
 * 
 * void *start - memory address to start at
 * void *end - memory address to end at
 * 
 * returns - mp_fp_t* to device if found or NULL on error
 */
mp_fp_t * find_mp_fp(void *start, void *end)
{
	void * addr;
	mp_fp_t *sd = NULL;
	unsigned char checksum, i;
	unsigned char *ptr;
	
	for (addr = start; addr <= end; addr+=4)
	{		
		sd = (mp_fp_t*)addr;
		
		if (strncmp(sd->signature, MP_SIGNATURE, 4) == 0)
		//if (sd->signature == ((('_' << 0) + ('3' << 8) + ('2' << 16) + ('_' << 24))))
		{			
			ptr = (unsigned char*)addr;
			checksum = 0;
			for (i = 0; i < sizeof(mp_fp_t); i++) 
				checksum += *(ptr++);
			
			if ((checksum == 0) && (sd->length == 1))
				return addr;
		}
	}
	
	return NULL;
}

/**
 * Gets the current interrupt mode: PIC, or APIC
 * 
 * returns - interrupt mode
 */
byte mp_get_intmode(void)
{
	return intmode;
}
